#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <cmath>
#include <string>
#include <Eigen/Eigen>
#include "visualization_msgs/Marker.h"

using namespace std;

#define TTY_PATH "/dev/tty"
#define STTY_US "stty raw -echo -F "
#define STTY_DEF "stty -raw echo -F "

// read from keyboard
int get_char()
{
    fd_set rfds;
    struct timeval tv;
    int ch = -1;

    FD_ZERO(&rfds);
    FD_SET(0, &rfds);
    tv.tv_sec = 0;
    tv.tv_usec = 0; //设置等待超时时间

    //检测键盘是否有输入
    if (select(1, &rfds, NULL, NULL, &tv) > 0)
    {
        ch = getchar();
    }
    return ch;
}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "up_planner_node");
    ros::NodeHandle nh("~");

    // useless
    float map_x_size_ = 30.0;
    float map_y_size_ = 30.0;
    float map_z_size_ = 5.0;
    // nh.param("map_size_x",map_x_size_,30.0);
    // nh.param("map_size_y",map_y_size_,30);
    // nh.param("map_size_z",map_z_size_,5);

    // srand((float)time(0.0));

    int k = 6;

    // ros::Timer set_pub_timer = nh.createTimer(ros::Duration(0.1),point_callback);
    ros::Publisher target_pub[k];
    ros::Publisher moving_target_pub = nh.advertise<visualization_msgs::Marker>("/moving", 10);
    ros::Publisher moving_target_pub_swarm = nh.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
    for (int i = 0; i < k; i++)
    {
        string target_name = string("/drone_") + to_string(i) + string("/moving_goal");
        target_pub[i] = nh.advertise<nav_msgs::Odometry>(target_name.c_str(), 10);
    }
    visualization_msgs::Marker marker_;
    marker_.ns = "my_namespace";
    marker_.header.stamp = ros::Time();
    marker_.header.frame_id = "world";
    marker_.type = visualization_msgs::Marker::SPHERE;
    marker_.scale.x = 0.5;
    marker_.scale.y = 0.5;
    marker_.scale.z = 0.5;
    /*decide the color of the marker*/
    marker_.color.a = 1.0; // Don't forget to set the alpha!
    marker_.color.r = 0.8;
    marker_.color.g = 0;
    marker_.color.b = 0;
    /*set marker action*/
    marker_.action = visualization_msgs::Marker::ADD;

    ros::Rate rate(1000);

    //发布圆轨迹
    double radius = 5;
    double omega = 0.3;
    // Eigen::Vector3d target_wp{5.2, -3.3, 0.0};
    Eigen::Vector3d target_wp{-20, 0.0, 0.0};
    Eigen::Vector3d target_vel{0.0, 0.0, 0.0};
    Eigen::Vector3d target_acl{0.0, 0.0, 0.0};
    //相对位置
    vector<vector<double>> rel_pose = {{-1.0, 0.0, 1.5}, {1.5, -1.5, 1.5}, {0, 1.5, 1.5}, {0, -1.5, 1.5}, {-1.5, 1.5, 1.5}, {-1.5, -1.5, 1.5}};
    // vector<vector<double>> rel_pose = {{-sqrt(3), -1.0, 1.5}, {-sqrt(3), 1.0, 1.5}, {0.0, 2.0, 1.5}, {sqrt(3), 1.0, 1.5}, {sqrt(3), -1.0, 1.5}, {0.0, -2.0, 1.5}};

    cout << "go!" << endl;
    int count = 0;
    while (ros::ok())
    {

        static float alpha = 0.0;
        static float x = 0.0;

        // target_acl(0) = 0;
        // target_acl(1) = 0;

        //直线
        // target[0] += 1.5 / 20.0;
        // target[0] = min(target[0], 22.0);

        //多项式
        // x+=1.0/20.0;
        // target[0]+= 1/20;
        // // target[1]=dmoinate(target[0]);

        //操控
        int ch = 0;
        system(STTY_US TTY_PATH);
        ch = get_char();
        switch (ch)
        {
        case 'w':
            target_acl(0) += 10;
            break;

        case 'a':
            target_acl(1) += 10;
            break;

        case 's':
            target_acl(0) -=10;
            break;

        case 'd':
            target_acl(1) -= 10;
            break;

        case 'q':
            target_acl(0) = 0;
            target_acl(1) = 0;
            break;

        case 'e':
            target_acl(0)=0;
            target_acl(1)=0;
            target_vel(0)=0;
            target_vel(1)=0;
            goto A;
            break;

        default:
            break;
        }
        if (ch == 'k')
        {
            system(STTY_DEF TTY_PATH);
            cout << "program end"
                 << "\r\n";
            return 0;
        }

        if (target_acl.norm() > 5)
        {
            target_acl = 5.0 / target_acl.norm() * target_acl;
        }
        target_vel += target_acl * 0.001;
        if (target_vel.norm() > 1.5)
        {
            target_vel = 2.1 / target_vel.norm() * target_vel;
        }
        A:cout << target_vel << "\r\n";

        target_wp += target_vel * 0.001 + 0.5 * target_acl * 0.001 * 0.001;

        //圆形
        // alpha+=omega/1000.0;
        // target_wp[0]=-radius*cos(alpha);
        // target_wp[1]=radius*sin(alpha);
        // target_vel[0]=radius*omega*sin(alpha);
        // target_vel[1]=radius*omega*cos(alpha);

        // target_wp[0]+=1.5/1000;

        marker_.pose.position.x = target_wp(0);
        marker_.pose.position.y = target_wp(1);
        marker_.pose.position.z = 0.0;
        moving_target_pub.publish(marker_);

        // cout<<target[0]<<"          "<<target[1]<<endl;
        if (count == 49)
        {
            nav_msgs::Odometry tmp;
            for (int i = 0; i < k; i++)
            {
                tmp.pose.pose.position.x = target_wp(0) + rel_pose[i][0];
                tmp.pose.pose.position.y = target_wp(1) + rel_pose[i][1];
                tmp.pose.pose.position.z = target_wp(2) + rel_pose[i][2];
                tmp.twist.twist.linear.x = target_vel[0];
                tmp.twist.twist.linear.y = target_vel[1];
                target_pub[i].publish(tmp);
            }

        geometry_msgs::PoseStamped tmp_swarm;
        tmp_swarm.pose.position.x=target_wp(0);
        tmp_swarm.pose.position.y=target_wp(1);
        tmp_swarm.pose.position.z=0.0;
        moving_target_pub_swarm.publish(tmp_swarm);

            count = 0;
        }
        else
            count++;

        // nav_msgs::Odometry tmp;
        // for (int i = 0; i < k; i++)
        // {
        //     tmp.pose.pose.position.x = target_wp(0) + rel_pose[i][0];
        //     tmp.pose.pose.position.y = target_wp(1) + rel_pose[i][1];
        //     tmp.pose.pose.position.z = target_wp(2) + rel_pose[i][2];
        //     tmp.twist.twist.linear.x = target_vel[0];
        //     tmp.twist.twist.linear.y = target_vel[1];
        //     target_pub[i].publish(tmp);
        // }

        rate.sleep();
    }

    return 0;
}